/*
 * speed.h
 *
 *  Created on: 2024年11月16日
 *      Author: zizhong
 */

#ifndef CONTROLLIB_SPEED_H_
#define CONTROLLIB_SPEED_H_

typedef struct {
       float ElecTheta;     // Input: Electrical angle (pu)
       uint32_t DirectionQep;   // Variable: Direction of rotation (Q0) - independently with global Q
       float OldElecTheta;  // History: Electrical angle at previous step (pu)
       float Speed;         // Output: Speed in per-unit  (pu)
       uint32_t BaseRpm;        // Parameter: Base speed in rpm (Q0) - independently with global Q
       float K1;            // Parameter: Constant for differentiator (Q21) - independently with global Q
       float K2;            // Parameter: Constant for low-pass filter (pu)
       float K3;            // Parameter: Constant for low-pass filter (pu)
       float SpeedRpm;        // Output : Speed in rpm  (Q0) - independently with global Q
       float Tmp;           //Variable: Temp variable
       } SPEED_MEAS_ABS_ENCODER;        // Data type created

#define SPEED_MEAS_ABS_ENCODER_DEFAULTS { 0, \
                                    1, \
                                    0, \
                                    0, \
                                    0, \
                                    0, \
                                    0, \
                                    0, \
                                    0, \
                                    0  \
                                  }

static inline float runSpeedFR(SPEED_MEAS_ABS_ENCODER * in)
{
    in->Tmp = in->ElecTheta - in->OldElecTheta;

    if(in->Tmp < -0.5f)
    {
        in->Tmp += 1.0f;
    }
    else if(in->Tmp > 0.5f)
    {
        in->Tmp -= 1.0f;
    }

    in->Tmp *= in->K1;

    // Low-pass filter
    in->Tmp = (in->K2 * in->Speed) + (in->K3 * in->Tmp);
    // Saturate the output

    in->Speed = __fsat(in->Tmp, 1.0f, -1.0f);

    // Update the electrical angle
    in->OldElecTheta = in->ElecTheta;

    // Change motor speed for pu to rpm value
    in->SpeedRpm = in->BaseRpm * in->Speed;
    return(in->Speed);
}

#endif /* CONTROLLIB_SPEED_H_ */
